
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pi_2d.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "pi_2d.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// calc_filt_alpha - recalculate the input filter alpha
void pi_2d_ctrl_calc_filt_alpha(PI_2d_ctrl * controler)
{
    if (math_flt_zero(controler->_filt_hz)) {
        controler->filt_alpha = 1.0f;
        return;
    }
  
    // calculate alpha
    const float rc = 1/(M_2PI*controler->_filt_hz);
    controler->filt_alpha = controler->dt / (controler->dt + rc);
}

void pi_2d_ctrl_init(PI_2d_ctrl * controler, float kp, float ki, float kimax, float filt_hz, float initial_dt)
{
    controler->_kp = kp;
    controler->_ki = ki;
    controler->_kimax = fabsf(kimax);
    controler->_filt_hz = fabsf(filt_hz);

    controler->dt = initial_dt;
    controler->input.x = 0;    
    controler->input.y = 0;
    
    // reset input filter to first value received
    controler->_flags_reset_filter = true;

}

// set_dt - set time step in seconds
void pi_2d_ctrl_set_dt(PI_2d_ctrl * controler, float dt)
{
    // set dt and calculate the input filter alpha
    controler->dt = dt;
    pi_2d_ctrl_calc_filt_alpha(controler);
}

// set_input - set input to PID controller
//  input is filtered before the PID controllers are run
//  this should be called before any other calls to get_p, get_i or get_d
void pi_2d_ctrl_set_input(PI_2d_ctrl * controler, Vector2f_t input)
{
    // don't process inf or NaN
    if (!isfinite(input.x) || !isfinite(input.y)) {
        return;
    }

    // reset input filter to value received
    if (controler->_flags_reset_filter) {
        controler->_flags_reset_filter = false;
        controler->input = input;
    }

    // update filter and calculate derivative
    Vector2f_t input_delta = {(input.x - controler->input.x) * controler->filt_alpha,
                                    (input.y - controler->input.y) * controler->filt_alpha};
    
    controler->input.x += input_delta.x;
    controler->input.y += input_delta.y;
}

// get_pi - get results from pid controller
Vector2f_t pi_2d_ctrl_get_pi(PI_2d_ctrl * controler)
{
    Vector2f_t pi = {controler->input.x * controler->_kp + controler->integrator.x,
                      controler->input.y * controler->_kp + controler->integrator.y};

    return pi;
}

Vector2f_t pi_2d_ctrl_get_p(PI_2d_ctrl * controler)
{
    Vector2f_t p = {controler->input.x * controler->_kp,
                    controler->input.y * controler->_kp};
    return p;
}

Vector2f_t pi_2d_ctrl_get_i(PI_2d_ctrl * controler)
{
    Vector2f_t i_tmp = {0,0};

    if (!math_flt_zero(controler->_ki) && !math_flt_zero(controler->dt)) {
        controler->integrator.x += (controler->input.x * controler->_ki) * controler->dt;
        controler->integrator.y += (controler->input.y * controler->_ki) * controler->dt;

        const float integrator_length =vec2_length(&controler->integrator);
        if ((integrator_length > controler->_kimax) && (math_flt_positive(integrator_length))) {
            controler->integrator.x *= (controler->_kimax / integrator_length);
            controler->integrator.y *= (controler->_kimax / integrator_length);
        }
        return controler->integrator;
    }
    return i_tmp;

}

Vector2f_t pi_2d_ctrl_get_i_shrink(PI_2d_ctrl * controler)   // get_i but do not allow integrator to grow (it may shrink)
{
    Vector2f_t i_shrink = {0,0};
    
    if (!math_flt_zero(controler->_ki) && !math_flt_zero(controler->dt)) {
        const float integrator_length_orig = MIN(vec2_length(&controler->integrator), controler->_kimax);
        
        controler->integrator.x += (controler->input.x * controler->_ki) * controler->dt;
        controler->integrator.y += (controler->input.y * controler->_ki) * controler->dt;

        const float integrator_length_new = vec2_length(&controler->integrator);
        if ((integrator_length_new > integrator_length_orig) && math_flt_positive(integrator_length_new)) {
            controler->integrator.x *= (integrator_length_orig / integrator_length_new);
            controler->integrator.y *= (integrator_length_orig / integrator_length_new);
        }
        return controler->integrator;
    }
    return i_shrink;
}

// reset_I - reset the integrator
void pi_2d_ctrl_reset_I(PI_2d_ctrl * controler)
{
    controler->integrator.x = 0;
    controler->integrator.y = 0;
}

void pi_2d_ctrl_set_filt_hz(PI_2d_ctrl * controler, float filt_hz)
{
    controler->_filt_hz = (fabsf(filt_hz));

    // sanity check _filt_hz
    controler->_filt_hz = MAX(controler->_filt_hz, PI_2D_FILT_HZ_MIN);

    // calculate the input filter alpha
    pi_2d_ctrl_calc_filt_alpha(controler);
}

void pi_2d_ctrl_set_integrator3d(PI_2d_ctrl * controler, Vector3f_t i)
{
    Vector2f_t i_2d = {i.x,i.y};

    controler->integrator = i_2d;
}

/*------------------------------------test------------------------------------*/


